#ifndef HB_APM_FLIGHT_HANDLER_H
#define HB_APM_FLIGHT_HANDLER_H
#include <iostream>
#include <fstream>
#include <chrono>
#include <algorithm>
#include <unistd.h>
#include <sys/stat.h>
#include <stdarg.h>
#include <string.h>
#include <map>
#include <mutex>
#include <vector>
#include <string>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <sys/file.h>
#include <sys/types.h>
#include <sys/statfs.h>
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>
#include <pthread.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/types.h>
#include <signal.h>
#include <termios.h>
#include <errno.h>
#include <string.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/wait.h>
#include <arpa/inet.h>
#include <assert.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <math.h>
#include <thread>
#include <sys/types.h>
#include <dirent.h>
//mavlink header file
#include  <ardupilotmega/mavlink.h>
#include  <common/mavlink.h>
#include  <uAvionix/mavlink.h>

#include <opencv2/opencv.hpp>
#include "circular_buf.h"
#include "mount_siyi.h"


#define M_PI      	(3.141592653589793f)
#define ToRad(x) 	(x * M_PI / 180)
#define ToDeg(x) 	(x * 180.0 / M_PI)

typedef struct Vector3f {
    float x;
    float y;
    float z;
} Vector3f;

typedef struct Vector3i {
    int x;
    int y;
    int z;
} Vector3i;


enum CopterMode{
	STABILIZE =     0,  // manual airframe angle with manual throttle
	ACRO =          1,  // manual body-frame angular rate with manual throttle
	ALT_HOLD =      2,  // manual airframe angle with automatic throttle
	AUTO =          3,  // fully automatic waypoint control using mission commands
	GUIDED =        4,  // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
	LOITER =        5,  // automatic horizontal acceleration with automatic throttle
	RTL =           6,  // automatic return to launching point
	CIRCLE =        7,  // automatic circular flight with automatic throttle
	LAND =          9,  // automatic landing with horizontal position control
	DRIFT =        11,  // semi-autonomous position, yaw and throttle control
	SPORT =        13,  // manual earth-frame angular rate control with manual throttle
	FLIP =         14,  // automatically flip the vehicle on the roll axis
	AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
	POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
	BRAKE =        17,  // full-brake using inertial/GPS system, no pilot input
	THROW =        18,  // throw to launch mode using inertial/GPS system, no pilot input
	AVOID_ADSB =   19,  // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
	GUIDED_NOGPS = 20,  // guided mode but only accepts attitude and altitude
	SMART_RTL =    21,  // SMART_RTL returns to home by retracing its steps
	FLOWHOLD  =    22,  // FLOWHOLD holds position with optical flow without rangefinder
	FOLLOW    =    23,  // follow attempts to follow another vehicle or ground station
	ZIGZAG    =    24,  // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
	SYSTEMID  =    25,  // System ID mode produces automated system identification signals in the controllers
	AUTOROTATE =   26,  // Autonomous autorotation
	AUTO_RTL =     27,  // Auto RTL, this is not a true mode, AUTO will report as this mode if entered to perform a DO_LAND_START Landing sequence
	TURTLE =       28,  // Flip over after crash
};

struct  BaseMode{
	bool m_customModeEnabledFlag:1;
	bool m_testEnabledFlag:1;
	bool m_autoEnabledFlag:1;
	bool m_guidedEnabledFlag:1;
	bool m_stabilizeEnabledFlag:1;
	bool m_hilEnabledFlag:1;
	bool m_manualInputEnabledFlag:1;
	bool m_safetyArmedFlag:1;
};



int64_t getTimeMillis();

void sleepMs(uint32_t timeMs);

double wrap360(const double angle);

class MountSiyi;


class apmFlightHandler
{
public:
	apmFlightHandler();

	~apmFlightHandler();

	bool startUpSystem(MountSiyi *mountSiyiHandler);

	int ttyConfigure(char *ttyPort, char *baudrate);

	//Subscribe to mavlink messages
	void subcribeMavlinkMessages();


	//mavlink recv and send
	void StartMavlinkMessagesSend(uint8_t message_rate, uint8_t stream_id);

	bool getMavlinkRunFlag();

	void setMavlinkRunFlag(bool status);

	void handlerMavlinkRecv();

	void handlerMavlinkSend();

	int packMavlinkData(mavlink_message_t *msg, uint8_t *buffer);

	//parse heartBeat msg
	void handlerHeartBeatMsg(mavlink_message_t &msg);

	void getFlightMode(uint32_t &flightMode, int64_t &updateTimeStamp);

	void getFlightArmStatus(bool &arm, int64_t &updateTimeStamp);

	void getFlightSystemStatus(MAV_STATE &systemStatus, int64_t &updateTimeStamp);

	//parse system time msg
	void handlerSystemTimeMsg(mavlink_message_t &msg);

	//parse gps raw msg
	void handlerGpsRawMsg(mavlink_message_t &msg);

	//parse attitude msg
	void handlerAttitudeMsg(mavlink_message_t &msg);

	void handlerIsLandStaus(mavlink_message_t &msg);

	void handlerGetFlightHome(mavlink_message_t &msg);

	void handlerPositionTargetLocalNED(mavlink_message_t &msg);

	void getFlightAttitude(Vector3f &flightAttitude, int64_t &updateTimeStamp);

	float getFlightCurrentYaw();

	//parse local position msg
	void handleLocalPositionNed(mavlink_message_t &msg);

	void getLocalPositon(Vector3f &localPosition, int64_t &updateTimeStamp);

	void getLocalVelocity(Vector3f &localVelocity, int64_t &updateTimeStamp);

	//parse global position msg
	void handlerGlobalPositionMsg(mavlink_message_t &msg);

	void getGlobalPosition(Vector3i &flightPosition, int64_t &updateTimeStamp);

	void getGlobalVelocity(Vector3f &flightVelocity, int64_t &updateTimeStamp);

	void getRelativeAlt(float &flightRelativeAlt, int64_t &updateTimeStamp);

	void getFlgihtHeading(float &flightHeading, int64_t &updateTimeStamp);

	void getLandStatus(MAV_LANDED_STATE &landStatus, int64_t &updateTimeStamp);

	void getFlightHome(Vector3f &home, int64_t &updateTimeStamp);

	void getTargetPositionDistance(Vector3f &distance, int64_t &updateTimeStamp);

	//parse command ack msg
	void handlerCommandAck(mavlink_message_t &msg);

	//control flight
	bool setFlightMode(uint32_t mode);

	//gimbal and camera
	//RETRACT
	bool commandDoGimbalManagerPitchyawRetract();

	//NEUTRAL mode
	bool commandDoGimbalManagerPitchyawNeutral();

	//YAW_LOCK
	bool commandDoGimbalManagerPitchyawYawLock(float pitchAngleDeg, float yawAngleDeg, float pitchRateDegs, float yawRateDegs);

	//log
	bool getLogDir();

	bool createLogFile();

	bool writeLogFile(const char * format, ...);

	bool clearAllLogFile();

	bool viewLogFile(std::vector<std::string> &logFileName);

	void recodeCpuTemperature();

	//liveView
	void setGimbalAttitude(float roll, float pitch, float yaw);

	void getGimbalAttitude(Vector3f &gimbalAttitude, int64_t &updateTimeStamp);

	void setLiveViewImage(cv::Mat image);

	void getLiveViewImage(cv::Mat &image, float &imageFlightYaw, Vector3f& imageGimbalAngle, int64_t &updateTimeStamp);

	bool getLiveViewHealthy();

	//guided mode control flight

	//z: is altitude in meters ; x is latitude (degrees * 10^7), y is  longitude (degrees * 10^7)
	// msg 75
	bool setPoistionControlFlight(int32_t latitude, int32_t longitude, float altitude);
	//msg 86
	bool setPoistionControlFlight2(int32_t latitude, int32_t longitude, float altitude);

	bool setLandPoistionToFlight();

	bool velocityControlToCopter(Vector3f velocity, double yawRate);


private:
	bool m_systemTimeSetFlag;
	//copter uart
	int m_ttyFd;
	bool m_connect;
	mavlink_status_t m_mavlinkStatus;
	std::mutex m_packMavlinkDataLock;

	uint32_t  m_flightMode;
	int64_t m_flightModeUpdateTimeStamp;
	std::mutex m_flightModeUpdateLock;

	//arm is true, disarm is false
	bool m_flightArm;
	int64_t m_flightArmUpdateTimeStamp;
	std::mutex  m_flightArmUpdateLock;

	//flight system status,
	//land_complete is true -->MAV_STATE_STANDBY, MAV_STATE_STANDBY is false -->MAV_STATE_ACTIVE
	//failsafe have triggered  -->MAV_STATE_CRITICAL
	MAV_STATE m_flightSystemStatus;
	int64_t m_flightSystemStatusUpdateTimeStamp;
	std::mutex m_flightSystemStatusUpdateLock;

	//gps
	GPS_FIX_TYPE m_gpsFixType;
	int64_t m_gpsFixTypeUpdateTimeStamp;
	std::mutex m_gpsFixTypeUpdateLock;

	//attitude :x, y, z  ---> pitch, roll, yaw
	Vector3f m_flightAttitude;
	int64_t m_flightAttitudeUpdateTimeStamp;
	std::mutex m_flightAttitudeUpdateLock;

	//local position msg
	Vector3f m_localPosition;
	int64_t m_localPositionUpdateTimeStamp;
	std::mutex m_localPositionUpdateLock;

	Vector3f m_localVelocity;
	int64_t m_localVelocityUpdateTimeStamp;
	std::mutex m_localVelocityUpdateLock;

	//global position x,y,z --->lat, lon, alt
	Vector3i m_flightPosition;
	int64_t m_flightPositionUpdateTimeStamp;
	std::mutex m_flightPositionUpdateLock;

	Vector3f m_flightVelocity;//m/s
	int64_t m_flightVelocityUpdateTimeStamp;
	std::mutex m_flightVelocityUpdateLock;

	float m_flightRelativeAlt; // m
	int64_t m_flightRelativeAltUpdateTimeStamp;
	std::mutex m_flightRelativeAltUpdateLock;

	float m_flightHeading;
	int64_t m_flightHeadingUpdateTimeStamp;
	std::mutex m_flightHeadingUpdateLock;

	MAV_LANDED_STATE m_landedState;
	int64_t m_landedStateUpdateTimeStamp;
	std::mutex m_landedStateUpdateLock;

	//x is latitude (deg), y is longitude (deg), z is altitude (m)
	Vector3f m_flightHome;
	int64_t m_flightHomeUpdateTimeStamp;
	std::mutex m_flightHomeUpdateLock;


	//command ack result
	MAV_RESULT m_setModeCommandResult;
	MAV_RESULT m_setCommandLongResult;
	MAV_RESULT m_setcommandInitResult;

	//mavlink
	struct circularBuffer m_mavlinkSendBuffer;

	pthread_t m_mavlinkRecvRunTid;
	pthread_t m_mavlinkSendRunTid;
	bool m_mavlinkRunFlag;

	//siyi gimbal
	MountSiyi *m_mountSiyiHandler;

	//gimbal attitude
	Vector3f m_siyiGimbalAttitude; // roll is x, pitch is y, yaw is z
	int64_t m_siyiGimbalAttitudeUpdateTimeStamp;
	std::mutex m_siyiGimbalAttitudeLock;

	//rtsp image
	cv::Mat m_siyiLiveView;
	float m_imageFlightYaw;
	Vector3f m_siyiImageGimbalAngle; // relative center position
	int64_t m_siyiLiveViewUpdateTimeStamp;
	std::mutex m_siyiLiveViewLock;

	//land point
	bool m_landPointOk;
	//z: is altitude in meters ; x is latitude (degrees * 10^7), y is  longitude (degrees * 10^7)
	int32_t m_landLatitude;
	int32_t m_landLongitude;
	float m_landAltitude;

	//target position distance, uint m
	Vector3f m_targetPostionDistance;
	int64_t m_targetPostionDistanceUpdateTimeStamp;
	std::mutex m_targetPostionDistanceLock;

	//log file
	FILE *m_logFp;
	bool m_getLogDirFlag;
	uint32_t m_lastLogNum;

	int m_attitudeLogCount;
};

extern apmFlightHandler flightHandler;

#endif //HB_APM_FLIGHT_HANDLER_H
